/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2022 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
<<<<<<< HEAD
#include <stdio.h>
=======
>>>>>>> b760fe4adf5cf0ff362c5f2d147df9832c8d5d95
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "pid.h"
#include "usart.h"
#include "can.h"
#include "visEffect.h"
#include "ws2812.h"
#include "math.h"
#include "ramp.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
typedef struct
{
    uint16_t ecd;
    int16_t speed_rpm;
    int16_t given_current;
    uint8_t temperate;
    int16_t last_ecd;
} motor_measure_t;
/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define CAN_3508_M1_ID  0x201
#define CAN_3508_M2_ID 0x202
#define CAN_3508_M3_ID 0x203
#define CAN_3508_M4_ID 0x204
#define get_motor_measure(ptr, data)                                    \
    {                                                                   \
        (ptr)->last_ecd = (ptr)->ecd;                                   \
        (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
        (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
        (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
        (ptr)->temperate = (data)[6];                                   \
    }
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
static CAN_TxHeaderTypeDef  chassis_tx_message;
static motor_measure_t motor_chassis;
<<<<<<< HEAD
float speed1=0.785f,speed2=1.884f;//0.785f 1.884f  1.305
=======
>>>>>>> b760fe4adf5cf0ff362c5f2d147df9832c8d5d95
float speed_fdb=0.0;
float speed_ref=0.0;
pid_type_def mypid;
float PIDCOEF[3];
int16_t current=0.0;
static uint8_t              chassis_can_send_data[8];
uint32_t send_mail_box;
float uartdata[5];
uint32_t xLastWakeTime;
int count=0;
float startflag=0;
int flag=1;
int lightflag=0;
<<<<<<< HEAD
float start_ramp_speed = 0;     //??????????????
float stop_ramp_speed = 0;      //????????????
uint8_t until_stop_flag = 0;
float diraction = 1;          //??????
float  diraction_last = 1;    //????????????
uint8_t toggle_stop_flag = 0; //????????????
float speed_when_stop = 0;    //????????????????????
uint8_t tempture = 0;          //??????
=======
float start_ramp_speed = 0;     //刚开始时的斜坡速度
float stop_ramp_speed = 0;      //停止时的斜坡速度
uint8_t until_stop_flag = 0;
float diraction = 1;          //方向系数
float  diraction_last = 1;    //上一次的方向系数
uint8_t toggle_stop_flag = 0; //翻转方向暂停标志
float speed_when_stop = 0;    //接收到停止信号时电机的速度
uint8_t tempture = 0;          //电机温度
>>>>>>> b760fe4adf5cf0ff362c5f2d147df9832c8d5d95
extern WS2812_Struct ws2812b;
extern uint8_t RedFrameBuffer[120];
extern uint8_t BlueFrameBuffer[120];
extern ramp_t stop_ramp;
extern ramp_t start_ramp;
<<<<<<< HEAD
extern uint16_t this_time_rx_len;
extern uint8_t receive_flag;

uint8_t* uRx_Data;
=======

>>>>>>> b760fe4adf5cf0ff362c5f2d147df9832c8d5d95
/* USER CODE END Variables */
osThreadId debugHandle;
osThreadId windmillHandle;
osThreadId RCoreLightHandle;

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
void init_color_buffer(uint8_t* buf,size_t size,uint8_t r,uint8_t g,uint8_t b)
{
    for(int i=0;i<size;i+=3)
    {
        buf[i]=r;
        buf[i+1]=g;
        buf[i+2]=b;
    }
}

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    CAN_RxHeaderTypeDef rx_header;
    uint8_t rx_data[8];

    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);

    switch (rx_header.StdId)
    {
        case CAN_3508_M1_ID:
        case CAN_3508_M2_ID:
        case CAN_3508_M3_ID:
        case CAN_3508_M4_ID:
        {
            get_motor_measure(&motor_chassis, rx_data);
            speed_fdb=motor_chassis.speed_rpm/1.0f;
            tempture = rx_data[6];
            break;
        }

        default:
        {
            break;
        }
    }
}
/* USER CODE END FunctionPrototypes */

void DebugTask(void const * argument);
void WindMillTask(void const * argument);
void RCoreLightTask(void const * argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/* GetIdleTaskMemory prototype (linked to static allocation support) */
void vApplicationGetIdleTaskMemory( StaticTask_t **ppxIdleTaskTCBBuffer, StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize );

/* USER CODE BEGIN GET_IDLE_TASK_MEMORY */
static StaticTask_t xIdleTaskTCBBuffer;
static StackType_t xIdleStack[configMINIMAL_STACK_SIZE];

void vApplicationGetIdleTaskMemory( StaticTask_t **ppxIdleTaskTCBBuffer, StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize )
{
  *ppxIdleTaskTCBBuffer = &xIdleTaskTCBBuffer;
  *ppxIdleTaskStackBuffer = &xIdleStack[0];
  *pulIdleTaskStackSize = configMINIMAL_STACK_SIZE;
  /* place for user code */
}
/* USER CODE END GET_IDLE_TASK_MEMORY */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* definition and creation of debug */
  osThreadDef(debug, DebugTask, osPriorityNormal, 0, 128);
  debugHandle = osThreadCreate(osThread(debug), NULL);

  /* definition and creation of windmill */
  osThreadDef(windmill, WindMillTask, osPriorityNormal, 0, 128);
  windmillHandle = osThreadCreate(osThread(windmill), NULL);

  /* definition and creation of RCoreLight */
  osThreadDef(RCoreLight, RCoreLightTask, osPriorityNormal, 0, 128);
  RCoreLightHandle = osThreadCreate(osThread(RCoreLight), NULL);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

}

/* USER CODE BEGIN Header_DebugTask */
/**
  * @brief  Function implementing the debug thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_DebugTask */
void DebugTask(void const * argument)
{
  /* USER CODE BEGIN DebugTask */
  /* Infinite loop */
<<<<<<< HEAD

  for(;;)
  {
    if(receive_flag)
    {
        if(this_time_rx_len == 1)
        {
            switch (uRx_Data[0])
            {
                case '0':
                    flag = 0;
                    speed_when_stop = speed_fdb;
                    ramp_init(&stop_ramp, 800);
                    count = 0;
                    break;
                case '1':
                    flag = 1;
                    ramp_init(&start_ramp, 800);
                    count = 0;
                    start_ramp_speed = 0;
                    break;
                case 'r':
                    lightflag = 0;
                    break;
                case 'b':
                    lightflag = 1;
                    break;
                case '+':
                    diraction = 1;
                    speed_when_stop = speed_fdb;
                    ramp_init(&stop_ramp, 800);
                    count = 0;
                    break;
                case '-':
                    diraction = -1;
                    speed_when_stop = speed_fdb;
                    //(&stop_ramp, 800);
                    count = 0;
                    break;

            }

        }
        else if (this_time_rx_len>1)
        {
            sscanf(uRx_Data, "%5f %5f", &speed1, &speed2);
        }
        receive_flag = 0;
    }

=======
  for(;;)
  {
      uint8_t uRx_Data;
      if(HAL_UART_Receive(&huart2, &uRx_Data, 1, 1000) == HAL_OK)
      {
          switch (uRx_Data) {
              case '0':
                  flag=0;
                  speed_when_stop = speed_fdb;
                  ramp_init(&stop_ramp,800);
                  count = 0;
                  break;
              case '1':
                  flag=1;
                  ramp_init(&start_ramp,800);
                  count = 0;
                  start_ramp_speed = 0;
                  break;
              case 'r':
                  lightflag=0;
                  break;
              case 'b':
                  lightflag=1;
                  break;
              case '+':
                  diraction = 1;
                  speed_when_stop = speed_fdb;
                  ramp_init(&stop_ramp,800);
                  count = 0;
                  break;
              case '-':
                  diraction = -1;
                  speed_when_stop = speed_fdb;
                  ramp_init(&stop_ramp,800);
                  count = 0;
                  break;
          }

      }
>>>>>>> b760fe4adf5cf0ff362c5f2d147df9832c8d5d95
      uartdata[0]=PIDCOEF[0];
      uartdata[1]=PIDCOEF[1];
      uartdata[2]=PIDCOEF[2];
      uartdata[3]=speed_ref;
      uartdata[4]=speed_fdb;;
      HAL_UART_Transmit(&huart2,(uint8_t *)uartdata,sizeof(uartdata),HAL_MAX_DELAY);
      osDelay(10);
  }
  /* USER CODE END DebugTask */
}

/* USER CODE BEGIN Header_WindMillTask */
/**
* @brief Function implementing the windmill thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_WindMillTask */
void WindMillTask(void const * argument)
{
  /* USER CODE BEGIN WindMillTask */
    CAN_FilterTypeDef can_filter_st;
    can_filter_st.FilterActivation = ENABLE;
    can_filter_st.FilterMode = CAN_FILTERMODE_IDMASK;
    can_filter_st.FilterScale = CAN_FILTERSCALE_32BIT;
    can_filter_st.FilterIdHigh = 0x0000;
    can_filter_st.FilterIdLow = 0x0000;
    can_filter_st.FilterMaskIdHigh = 0x0000;
    can_filter_st.FilterMaskIdLow = 0x0000;
    can_filter_st.FilterBank = 0;
    can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;

    HAL_CAN_ConfigFilter(&hcan1, &can_filter_st);
    HAL_CAN_Start(&hcan1);
    HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
    can_filter_st.SlaveStartFilterBank = 14;
    can_filter_st.FilterBank = 14;
    chassis_tx_message.StdId = 0x200;
    chassis_tx_message.IDE = CAN_ID_STD;
    chassis_tx_message.RTR = CAN_RTR_DATA;
    chassis_tx_message.DLC = 0x08;
    PIDCOEF[0]=11.0f;
    PIDCOEF[1]=0.5f;
    PIDCOEF[2]=0.0f;
    PID_init(&mypid,PID_POSITION,(float *)PIDCOEF,16000,5000);
<<<<<<< HEAD
    ramp_init(&stop_ramp,800); //?????
    ramp_init(&start_ramp,800);//?????
=======
    ramp_init(&stop_ramp,800); //关斜坡
    ramp_init(&start_ramp,800);//开斜坡
>>>>>>> b760fe4adf5cf0ff362c5f2d147df9832c8d5d95
    xLastWakeTime = xTaskGetTickCount();
  /* Infinite loop */
  for(;;)
  {
      if(flag==1&&toggle_stop_flag!=1) {

<<<<<<< HEAD
          if (diraction == diraction_last) //??????????
          {
              speed_ref = diraction * (368.763f * (speed1 * sin(count / 100.0f * speed2) + (2.09-speed1)) / 0.10472f);
              diraction_last = diraction;
              if(count==0) //????????????????????
              {
                  //??????????????
=======
          if (diraction == diraction_last) //如果方向一致
          {
              speed_ref = diraction * (368.763f * (0.785f * sin(count / 100.0f * 1.884f) + 1.305) / 0.10472f);
              diraction_last = diraction;
              if(count==0) //如果是第一次启动则斜坡启动
              {
                  //从停止后反向刚开始
>>>>>>> b760fe4adf5cf0ff362c5f2d147df9832c8d5d95
                  if (toggle_stop_flag==2)
                  {
                      if (start_ramp_speed!=speed_ref)
                      {
                          start_ramp_speed = speed_ref*ramp_calc(&start_ramp);
                          current=PID_calc(&mypid,speed_fdb,start_ramp_speed);
                          chassis_can_send_data[0]=current >> 8;
                          chassis_can_send_data[1]=current;
                          chassis_can_send_data[2]=current >> 8;
                          chassis_can_send_data[3]=current;
                          chassis_can_send_data[4]=current >> 8;
                          chassis_can_send_data[5]=current;
                          chassis_can_send_data[6]=current >> 8;
                          chassis_can_send_data[7]=current;
                          HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
                      }
                      else
                      {
                          toggle_stop_flag = 0;
                      }

                  }
<<<<<<< HEAD
                  //??????????
                  else
                  {
                      //??????????????????
=======
                  //从上电刚开始
                  else
                  {
                      //等待斜坡速度达到目标速度
>>>>>>> b760fe4adf5cf0ff362c5f2d147df9832c8d5d95
                      if (start_ramp_speed!=speed_ref)
                      {
                          start_ramp_speed = speed_ref*ramp_calc(&start_ramp);
                          current=PID_calc(&mypid,speed_fdb,start_ramp_speed);
                          chassis_can_send_data[0]=current >> 8;
                          chassis_can_send_data[1]=current;
                          chassis_can_send_data[2]=current >> 8;
                          chassis_can_send_data[3]=current;
                          chassis_can_send_data[4]=current >> 8;
                          chassis_can_send_data[5]=current;
                          chassis_can_send_data[6]=current >> 8;
                          chassis_can_send_data[7]=current;
                          HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
                      }
                      else
                      {
                          count++;

                      }
                  }


              }

              else
              {
                  current=PID_calc(&mypid,speed_fdb,speed_ref);
                  chassis_can_send_data[0]=current >> 8;
                  chassis_can_send_data[1]=current;
                  chassis_can_send_data[2]=current >> 8;
                  chassis_can_send_data[3]=current;
                  chassis_can_send_data[4]=current >> 8;
                  chassis_can_send_data[5]=current;
                  chassis_can_send_data[6]=current >> 8;
                  chassis_can_send_data[7]=current;
                  HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
                  count++;
              }
          }
<<<<<<< HEAD
          else //????????
=======
          else //如果不一致
>>>>>>> b760fe4adf5cf0ff362c5f2d147df9832c8d5d95
          {
              toggle_stop_flag = 1;
          }

      }
<<<<<<< HEAD
      //???0
      else if (flag==0 | toggle_stop_flag==1)       //????????? ??????? ??????????????
=======
      //收到0
      else if (flag==0 | toggle_stop_flag==1)       //收到停止信号 或者翻转 进行缓缓停止的操作
>>>>>>> b760fe4adf5cf0ff362c5f2d147df9832c8d5d95
      {
          stop_ramp_speed = speed_when_stop*(1-ramp_calc(&stop_ramp));
          current = PID_calc(&mypid,speed_fdb,stop_ramp_speed);
          chassis_can_send_data[0]=current >> 8;
          chassis_can_send_data[1]=current;
          chassis_can_send_data[2]=current >> 8;
          chassis_can_send_data[3]=current;
          chassis_can_send_data[4]=current >> 8;
          chassis_can_send_data[5]=current;
          chassis_can_send_data[6]=current >> 8;
          chassis_can_send_data[7]=current;
          HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
          if(stop_ramp_speed==0&&toggle_stop_flag==1)
          {
              diraction_last = diraction;
<<<<<<< HEAD
              toggle_stop_flag = 2;  //??????????????
=======
              toggle_stop_flag = 2;  //进入到反向启动模式
>>>>>>> b760fe4adf5cf0ff362c5f2d147df9832c8d5d95
              count = 0;
              flag = 1;
              ramp_init(&start_ramp,800);
          }

      }

      osDelayUntil(&xLastWakeTime, 10);
  }
  /* USER CODE END WindMillTask */
}

/* USER CODE BEGIN Header_RCoreLightTask */
/**
* @brief Function implementing the RCoreLight thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_RCoreLightTask */
void RCoreLightTask(void const * argument)
{
  /* USER CODE BEGIN RCoreLightTask */
    init_color_buffer(BlueFrameBuffer,sizeof(BlueFrameBuffer),0,0,0xff);
    init_color_buffer(RedFrameBuffer,sizeof(RedFrameBuffer),0xff,0,0);

  /* Infinite loop */
  for(;;)
  {
      uint8_t FrameRx_Data;
          if(lightflag==0)
          {
              ws2812b.item[0].frameBufferPointer = RedFrameBuffer;
          }

          if(lightflag==1) {
              ws2812b.item[0].frameBufferPointer = BlueFrameBuffer;
          }
      //visInit();
      visHandle();

    osDelay(200);
  }
  /* USER CODE END RCoreLightTask */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */

/* USER CODE END Application */
